/*
*********************************************************************************************************
*
*	模块名称 : USART调试串口驱动模块
*	文件名称 : bsp_usart.c
*	版    本 : V1.0
*	说    明 : 调试串口重定向
*
*	修改记录 :
*		版本号  日期        作者     说明
*		V1.0    2024-12-06 astronG  正式发布
*
*	Copyright (C), 2015-2030, astronG
*
*********************************************************************************************************
*/

#include "bsp.h"
#include "stm32f1xx_hal.h"

// 定义BNO055的I2C地址 (7位地址左移1位)
#define BNO055_I2C_ADDR (0x28 << 1) // 如果COM3接低电平；若接高电平，则使用 (0x29 << 1)


// 定义BNO055芯片ID寄存器地址


// 假设ICM - 20948的7位地址是0x68
#define ICM20948_ADDRESS (0x68 << 1)
// 加速度计数据起始寄存器地址
#define ACCEL_DATA_REG 0x3B

uint16_t dma_last_pos = 0;
//static volatile uint16_t dma_current_pos = 0;
uint16_t dma_current_pos = 0;
uint8_t uart_rx_buffer[UART_RX_BUFFER_SIZE];


/*
*********************************************************************************************************
*	函 数 名: debug_print
*	功能说明: 格式化并通过UART发送调试信息。该函数接受一个格式化字符串和可变数量的参数，
*            将它们格式化为一个字符串，然后通过UART发送出去。
*	形    参：
*           fmt: 格式化字符串，类似于debug_print的格式。
*           ...: 可变数量的参数，与fmt中的格式说明符对应。
*	返 回 值: 无
* 注意事项：传入参数不要超过255个字节，否则会导致数据丢失。
*						本函数进行了线程安全保护。(不要在中断调用本函数)
*********************************************************************************************************
*/
void debug_print(const char *const fmt, ...)
{
    char str[256];
    uint16_t len;
    va_list args;
    memset(str, 0, sizeof(str));
    va_start(args, fmt);
    vsnprintf(str, 255, fmt, args);
    va_end(args);
    len = strlen(str);
    // 获取互斥锁，保护串口资源
    xSemaphoreTake(xUartMutex, portMAX_DELAY);
    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)str, len);
    // 等待发送完成信号量
    xSemaphoreTake(xTxSemaphore, portMAX_DELAY);
    // 释放互斥锁
    xSemaphoreGive(xUartMutex);
    //    if(xSemaphoreTake(xUartMutex, pdMS_TO_TICKS(100))) {
    //        // 等待DMA空闲
    //        if(xSemaphoreTake(xTxSemaphore, pdMS_TO_TICKS(100))) {
    //            HAL_UART_Transmit_DMA(&huart1, (uint8_t*)str, len);
    //        } else {
    //            // 超时处理
    //        }
    //        xSemaphoreGive(xUartMutex);
    //    }
}

/*
*********************************************************************************************************
*	函 数 名: HAL_UART_TxCpltCallback
*	功能说明: UART发送完成回调函数
*	形    参：
*           huart: UART句柄
*	返 回 值: 无
*********************************************************************************************************
*/
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART1) {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        // 释放发送完成信号量
        xSemaphoreGiveFromISR(xTxSemaphore, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}



/*
*********************************************************************************************************
*	函 数 名: UART_IdleHandler
*	功能说明: 串口空闲中断处理函数
*	形    参: huart - 串口句柄
*	返 回 值: 无
*********************************************************************************************************
*/
void UART_IdleHandler(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART1) {
        __HAL_UART_CLEAR_IDLEFLAG(huart); // 清除空闲中断标志
        // 记录当前DMA位置
        dma_current_pos = UART_RX_BUFFER_SIZE - __HAL_DMA_GET_COUNTER(huart->hdmarx);
        // 释放信号量通知任务有数据到达
        xSemaphoreGiveFromISR(xUartRxSemaphore, NULL);
    }
}

/*
*********************************************************************************************************
*	函 数 名: ProcessUartData
*	功能说明: 处理接收到的串口数据，检测帧头帧尾
*	形    参: start_index - 数据起始索引
*           length - 数据长度
*	返 回 值: 无
*********************************************************************************************************
*/
void ProcessUartData(uint16_t start_index, uint16_t length)
{
    static uint8_t frame_state = 0;
    static uint16_t frame_length = 0;
    static UartFrame frame;  // 静态帧结构体

    for (uint16_t i = 0; i < length; i++) {
        uint16_t buffer_index = (start_index + i) % UART_RX_BUFFER_SIZE;
        uint8_t data_byte = uart_rx_buffer[buffer_index];

        // 检查缓冲区溢出
        if (frame_length >= UART_MAX_FRAME_SIZE) {
            frame_state = 0;
            frame_length = 0;
            memset(&frame, 0, sizeof(UartFrame));
            continue;
        }

        // 存储所有字节（包括帧头和帧尾）
        frame.data[frame_length++] = data_byte;

        // 状态机处理
        switch (frame_state) {
        case 0:
            if (data_byte == 0x55) {
                frame_state = 1;
            }

            break;

        case 1:
            frame_state = (data_byte == 0xAA) ? 2 : 0;
            break;

        case 2:
            frame_state = (data_byte == 0x55) ? 3 : 0;
            break;

        case 3:
            frame_state = (data_byte == 0xAA) ? 4 : 0;
            break;

        case 4:
            if (data_byte == 0x16) {
                frame_state = 5;
            }

            break;

        case 5:
            frame_state = (data_byte == 0xE9) ? 6 : 4;
            break;

        case 6:
            frame_state = (data_byte == 0x16) ? 7 : 4;
            break;

        case 7:
            if (data_byte == 0xE9) {
                // 完整帧接收完成
                frame.length = frame_length;

                // 将帧发送到队列
                if (xQueueSend(xUartFrameQueue, &frame, 0) != pdTRUE) {
                    debug_print("Frame queue full, frame discarded\r\n");
                }

                frame_state = 0;
                frame_length = 0;
                memset(&frame, 0, sizeof(UartFrame));
            } else {
                frame_state = 4;  // 回到数据状态
            }

            break;

        default:
            frame_state = 0;
            frame_length = 0;
            memset(&frame, 0, sizeof(UartFrame));
            break;
        }

        // 状态错误时重置
        if (frame_state == 0 && frame_length > 0) {
            frame_length = 0;
            memset(&frame, 0, sizeof(UartFrame));
        }
    }
}

/*
*********************************************************************************************************
*	函 数 名: ProcessUartFrame
*	功能说明: 处理完整的串口帧
*	形    参: frame - 帧数据
*	返 回 值: 无
*********************************************************************************************************
*/
void ProcessUartFrame(UartFrame *frame)
{
    // 直接提取指令部分（帧头帧尾已确认正确）
    uint8_t address = frame->data[4];  // 第5字节：指令类型
    uint8_t subcmd = frame->data[5];   // 第6字节：子指令
//    UartFrame response;
	float gyro[3], accel[3];

    // 指令处理
    switch (address) {
    case 0xFF:  // 示例指令
        switch (subcmd) {
        case 0xA1:
//			BNO055_CalibrateAccelerometer(&hi2c1);
//            BuildResponseFrame(&response);
//            SafeUartTransmit(response.data, response.length);
		BNO055_DebugReadIMUForFASTLIO(&hi2c1, gyro, accel);
            break;

        case 0xA2:
		CheckCalibrationStatus();
		BNO055_Diagnostic(&hi2c1);
            break;

        default:
			VerifyFASTLIODataQuality();
//            debug_print("Unknown subcmd: %02X\r\n", subcmd);
            break;
        }

        break;

    default:
        debug_print("Unknown address: %02X\r\n", address);
        break;
    }
}
/**
 * @brief 发送紧凑格式的IMU数据（使用int16_t减少数据量）
 */
void SendCompactFrameData(void)
{
    // 帧结构：帧头(2) + 时间戳(4) + 陀螺仪(6) + 加速度(6) + 帧尾(2) + 校验和(2) = 22字节
    uint8_t tx_buffer[22];
    uint32_t timestamp = HAL_GetTick();
    uint16_t checksum = 0;
    
    // 帧头
    tx_buffer[0] = 0xAA;
    tx_buffer[1] = 0xBB;
    
    // 时间戳
    memcpy(&tx_buffer[2], &timestamp, 4);
    
    // 陀螺仪数据（转换为int16_t，精度足够）
    int16_t gyro1_int = (int16_t)(BNO055_Data.gyro1 * 1000.0f); // 放大1000倍保持精度
    int16_t gyro2_int = (int16_t)(BNO055_Data.gyro2 * 1000.0f);
    int16_t gyro3_int = (int16_t)(BNO055_Data.gyro3 * 1000.0f);
    
    memcpy(&tx_buffer[6], &gyro1_int, 2);
    memcpy(&tx_buffer[8], &gyro2_int, 2);
    memcpy(&tx_buffer[10], &gyro3_int, 2);
    
    // 加速度数据（直接使用原始值，已经是int16_t）
    memcpy(&tx_buffer[12], &BNO055_Data.AccX, 2);
    memcpy(&tx_buffer[14], &BNO055_Data.AccY, 2);
    memcpy(&tx_buffer[16], &BNO055_Data.AccZ, 2);
    
    // 帧尾
    tx_buffer[18] = 0xCC;
    tx_buffer[19] = 0xDD;
    
    // 计算校验和
    for(int i = 0; i < 20; i++) {
        checksum += tx_buffer[i];
    }
    memcpy(&tx_buffer[20], &checksum, 2);
    
    SafeUartTransmit(tx_buffer, 22);
}
/**
 * @brief 发送FAST-LIO兼容的二进制数据帧
 */
void SendFASTLIOBinaryFrame(void)
{
    // 帧结构：帧头(2) + 时间戳(4) + 角速度(12) + 加速度(12) = 30字节
    uint8_t tx_buffer[30];
    uint32_t timestamp = HAL_GetTick();
    
    // 帧头标识
    tx_buffer[0] = 'I';
    tx_buffer[1] = 'M';
    tx_buffer[2] = 'U';
    
    // 时间戳
    memcpy(&tx_buffer[3], &timestamp, 4);
    
    // 角速度数据 (rad/s)
    memcpy(&tx_buffer[7], &BNO055_Data.gyro1, 4);
    memcpy(&tx_buffer[11], &BNO055_Data.gyro2, 4);
    memcpy(&tx_buffer[15], &BNO055_Data.gyro3, 4);
    
    // 加速度数据 (m/s²)
    float accel[3] = {
        BNO055_Data.AccX / 100.0f,
        BNO055_Data.AccY / 100.0f, 
        BNO055_Data.AccZ / 100.0f
    };
    
    memcpy(&tx_buffer[19], &accel[0], 4);
    memcpy(&tx_buffer[23], &accel[1], 4);
    memcpy(&tx_buffer[27], &accel[2], 4);
    
    SafeUartTransmit(tx_buffer, 31); // 31字节（包含帧头3字节）
}

void SafeUartTransmit(uint8_t *data, uint16_t length)
{
    if (xSemaphoreTake(xUartMutex, pdMS_TO_TICKS(50))) {
        //        if(xSemaphoreTake(xTxSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
        //            HAL_UART_Transmit_DMA(&huart1, data, length);
        //        }
        HAL_UART_Transmit_DMA(&huart1, data, length);
        xSemaphoreGive(xUartMutex);
    }
}

// 辅助函数：构建响应帧
void BuildResponseFrame(UartFrame *response)
{
    response->data[0] = BNO055_Data.AccX;  // FRAME_HEADER_1
    response->data[1] = BNO055_Data.AccY;  // FRAME_HEADER_2
    response->data[2] = BNO055_Data.AccZ;  // FRAME_HEADER_3
    response->data[3] = 0xAA;  // FRAME_HEADER_4
    response->data[4] = BNO055_Data.GyroX;  // 地址
    response->data[5] = BNO055_Data.GyroY;   // 命令
    response->data[6] = BNO055_Data.GyroZ;    // 数据高字节
    response->data[7] = 0xAA;  // 数据低字节
    response->data[8] = BNO055_Data.quat1;  // FRAME_FOOTER_1
    response->data[9] = BNO055_Data.quat2;  // FRAME_FOOTER_2
    response->data[10] = BNO055_Data.quat3; // FRAME_FOOTER_3
    response->data[11] = BNO055_Data.quat4; // FRAME_FOOTER_4
    response->length = 12;
}



/**
 * @brief 读取BNO055校准状态
 * @param hi2c I2C句柄
 * @return 校准状态字节
 */
//uint8_t BNO055_ReadCalibStatus(void)
//{
//    uint8_t calib_status;
//    HAL_I2C_Mem_Read(&hi2c1, BNO055_I2C_ADDR, 0x35,
//                     I2C_MEMADD_SIZE_8BIT, &calib_status, 1, HAL_MAX_DELAY);
//    return calib_status;
//}

// 确保设置为融合模式（如NDOF）
void BNO055_SetNDOFMode(void)
{
    uint8_t config_mode = 0x00;
    uint8_t ndof_mode = 0x0C;
    // Switch to configuration mode first
    HAL_I2C_Mem_Write(&hi2c1, BNO055_I2C_ADDR, 0x3D,
                      I2C_MEMADD_SIZE_8BIT, &config_mode, 1, HAL_MAX_DELAY);
    HAL_Delay(20);
    // Switch to NDOF mode
    HAL_I2C_Mem_Write(&hi2c1, BNO055_I2C_ADDR, 0x3D,
                      I2C_MEMADD_SIZE_8BIT, &ndof_mode, 1, HAL_MAX_DELAY);
    HAL_Delay(30);
    debug_print("Set to NDOF mode\n");
}

void SendCompactFrameData_FixedPoint(void)
{
    uint8_t tx_buffer[16]; // 进一步减少缓冲区
    
    // 帧头
    tx_buffer[0] = 0xAA;
    tx_buffer[1] = 0xBB;
    
    // 时间戳（低16位）
    uint32_t timestamp = HAL_GetTick();
    tx_buffer[2] = timestamp & 0xFF;
    tx_buffer[3] = (timestamp >> 8) & 0xFF;
    
    // 陀螺仪数据（int16_t原始值）
    tx_buffer[4] = BNO055_Data.GyroX & 0xFF;
    tx_buffer[5] = (BNO055_Data.GyroX >> 8) & 0xFF;
    tx_buffer[6] = BNO055_Data.GyroY & 0xFF;
    tx_buffer[7] = (BNO055_Data.GyroY >> 8) & 0xFF;
    tx_buffer[8] = BNO055_Data.GyroZ & 0xFF;
    tx_buffer[9] = (BNO055_Data.GyroZ >> 8) & 0xFF;
    
    // 加速度数据
    tx_buffer[10] = BNO055_Data.AccX & 0xFF;
    tx_buffer[11] = (BNO055_Data.AccX >> 8) & 0xFF;
    tx_buffer[12] = BNO055_Data.AccY & 0xFF;
    tx_buffer[13] = (BNO055_Data.AccY >> 8) & 0xFF;
    tx_buffer[14] = BNO055_Data.AccZ & 0xFF;
    tx_buffer[15] = (BNO055_Data.AccZ >> 8) & 0xFF;
    
    // 直接使用阻塞发送，避免DMA和互斥锁
    HAL_UART_Transmit(&huart1, tx_buffer, 16, 100);
}
